#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <filters.hpp>
#include <iostream>
#include <polynomial.hpp>

namespace scipy {
namespace signal {
/// @brief The factorial of a number or array of numbers.
/// The factorial of non-negative integer n is the product of all
/// positive integers less than or equal to n:
/// n! = n * (n - 1) * (n - 2) * ... * 1
/// @param n
/// @return
inline int factorial(int n)
{
    int fact = 1;
    for (int i = 1; i <= n; i++)
    {
        fact *= i;
    }
    return fact;
}
#ifndef savgol_print_matrix
#define savgol_print_matrix(x)                                                                     \
    do                                                                                             \
    {                                                                                              \
        Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");        \
        std::cout << #x << " " << __func__ << ":" << __LINE__ << " " << x.rows() << ","            \
                  << x.cols() << std::endl;                                                        \
        std::cout << x.format(HeavyFmt) << std::endl;                                              \
    } while (false)
#endif
/// @brief Given an N-d array `x` and the specification of a slice of `x` from
/// `window_start` to `window_stop` along `axis`, create an interpolating
/// polynomial of each 1-D slice, and evaluate that polynomial in the slice
/// from `interp_start` to `interp_stop`. Put the result into the
/// corresponding slice of `y`.
/// @param x
/// @param window_start
/// @param window_stop
/// @param interp_start
/// @param interp_stop
/// @param polyorder
/// @param deriv
/// @param delta
/// @param y
inline void fit_edge(Eigen::RowVectorXd x,
                     int window_start,
                     int window_stop,
                     int interp_start,
                     int interp_stop,
                     int polyorder,
                     int deriv,
                     float delta,
                     Eigen::RowVectorXd &y)
{
    // Get the edge into a(window_length, -1) array.
    // x_edge = axis_slice(x, start = window_start, stop = window_stop, axis = axis);
    Eigen::RowVectorXd x_edge = x.segment(window_start, window_stop - window_start);
    Eigen::VectorXd xx_edge   = x_edge;
    bool swapped              = false;
    // Fit the edges.poly_coeffs has shape(polyorder + 1, -1),
    // where '-1' is the same as in xx_edge.
    Eigen::VectorXd poly_coeffs =
        numpy::polyfit(Eigen::RowVectorXd::LinSpaced(window_stop - window_start,
                                                     0,
                                                     window_stop - window_start - 1),
                       xx_edge,
                       polyorder);

    if (deriv > 0)
    {
        poly_coeffs = numpy::polyder(poly_coeffs, deriv);
    }

    // # Compute the interpolated values for the edge.
    auto i = Eigen::VectorXd::LinSpaced(interp_stop - interp_start,
                                        interp_start - window_start,
                                        interp_stop - window_start - 1);

    Eigen::RowVectorXd values = numpy::polyval(poly_coeffs, i) / (std::pow(delta, deriv));

    // # Now put the values into the appropriate slice of y.
    // # First reshape values to match y.
    // shp = list(y.shape)
    // shp[0], shp[axis] = shp[axis], shp[0]
    // values = values.reshape(interp_stop - interp_start, *shp[1:])
    // if swapped:
    //     values = values.swapaxes(0, axis)
    // # Get a view of the data to be replaced by values.
    // auto y_edge = y.segment(interp_start, interp_stop - interp_start - 1);
    for (int i = interp_start; i < std::min(int(y.size()), interp_stop); i++)
    {
        y(i) = values(i - interp_start);
    }
}

/// @brief Use polynomial interpoly_edgeation of x at the low and high ends of the axis
/// to fill in the halflen values in y.
/// This function just calls _fit_edge twice, once for each end of the axis.
/// @param x
/// @param window_length
/// @param polyorder
/// @param deriv
/// @param delta
/// @param y
inline void fit_edges_polyfit(Eigen::RowVectorXd x,
                              int window_length,
                              int polyorder,
                              int deriv,
                              float delta,
                              Eigen::RowVectorXd &y)
{
    int halflen = window_length / 2;
    fit_edge(x, 0, window_length, 0, halflen, polyorder, deriv, delta, y);
    int n = x.size();
    fit_edge(x, n - window_length, n, n - halflen, n, polyorder, deriv, delta, y);
}

/// @brief Compute the coefficients for a 1-D Savitzky-Golay FIR filter.
/// References
/// https://github.com/scipy/scipy.git scipy/signal/_savitzky_golay.py 0.14.0
///
/// A. Savitzky, M. J. E. Golay, Smoothing and Differentiation of Data by
/// Simplified Least Squares Procedures. Analytical Chemistry, 1964, 36 (8),
/// pp 1627-1639.
///
/// Jianwen Luo, Kui Ying, and Jing Bai. 2005. Savitzky-Golay smoothing and
/// differentiation filter for even number data. Signal Process.
/// 85, 7 (July 2005), 1429-1434.
/// @param  window_length
/// The length of the filter window (i.e., the number of coefficients).
/// @param  polyorder
/// The order of the polynomial used to fit the samples.
/// `polyorder` must be less than `window_length`.
/// @param  deriv
/// The order of the derivative to compute. This must be a
/// nonnegative integer. The default is 0, which means to filter
/// the data without differentiating
/// @param delta
/// The spacing of the samples to which the filter will be applied.
/// This is only used if deriv > 0.
/// @param pos
/// If pos is not nullptr, it specifies evaluation position within the
/// window. The default is the middle of the window.
/// @param use
/// Either 'conv' or 'dot'. This argument chooses the order of the
/// coefficients. The default is 'conv', which means that the
/// coefficients are ordered to be used in a convolution. With
/// use='dot', the order is reversed, so the filter is applied by
/// dotting the coefficients with the data set.
/// @return coeffs
/// 1-D ndarray, The filter coefficients.
inline Eigen::RowVectorXd savgol_coeffs(int window_length,
                                        int polyorder,
                                        int deriv       = 0,
                                        float delta     = 1.0,
                                        int *pos        = nullptr,
                                        std::string use = "conv")
{

    eigen_assert((window_length > polyorder) &&
                 "transposeInPlace() called on a non-square non-resizable "
                 "matrix");
    if (polyorder >= window_length)
    {
        return {};
    }
    int halflen = window_length / 2;
    int rem     = window_length % 2;
    float fpos  = halflen;
    if (pos == nullptr)
    {
        if (rem == 0)
        {
            fpos = halflen - 0.5f;
        }
        else
        {
            fpos = halflen;
        }
    }
    else
    {
        fpos = *pos;
    }
    eigen_assert(((0 <= fpos) && (fpos < window_length)) && "pos must be nonnegative and less than "
                                                            "window_length.");
    if (!((0 <= fpos) && (fpos < window_length)))
    {
        return {};
    }
    const auto use_array = std::array<std::string, 2>{"conv", "dot"};
    bool use_in_flag     = std::find(use_array.begin(), use_array.end(), use) != use_array.end();
    eigen_assert(use_in_flag && "pos must be nonnegative and less than "
                                "window_length.");
    if (!use_in_flag)
    {
        return {};
    }

    if (deriv > polyorder)
    {
        return Eigen::VectorXd::Zero(window_length);
    }

    Eigen::RowVectorXd x =
        Eigen::VectorXd::LinSpaced(window_length, -fpos, window_length - fpos - 1);
    if (use == "conv")
    {
        x.reverseInPlace();
    }

    Eigen::VectorXi order = Eigen::VectorXi::LinSpaced(polyorder + 1, 0, polyorder);

    // python A = x ** order
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(order.rows(), x.cols());
    for (int r = 0; r < order.rows(); r++)
    {
        A.row(r) = x.array().pow(order(r, 0));
    }

    // y determines which order derivative is returned.
    Eigen::VectorXd y = Eigen::VectorXd::Zero(polyorder + 1);
    // The coefficient assigned to y[deriv] scales the result to take into
    // account the order of the derivative and the sample spacing.
    y(deriv) = factorial(deriv) / (std::pow(delta, deriv));
    // Find the least-squares solution of A*c = y
    Eigen::RowVectorXd coeffs = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y);
    return coeffs;
}

/// @brief Apply a Savitzky-Golay filter to an array.
/// This is a 1-D filter. Input type is RowVector, so remove axis param
/// @param x array_like
/// The data to be filtered. If `x` is not a single or double precision
/// floating point array, it will be converted to type ``numpy.float64``
/// before filtering.
/// @param window_length int
/// The length of the filter window (i.e., the number of coefficients).
/// If `mode` is 'interp', `window_length` must be less than or equal
/// to the size of `x`.
/// @param polyorder int
/// The order of the polynomial used to fit the samples.
/// `polyorder` must be less than `window_length`.
/// @param deriv int, optional
/// The order of the derivative to compute. This must be a
/// nonnegative integer. The default is 0, which means to filter
/// the data without differentiating.
/// @param delta float, optional
/// The spacing of the samples to which the filter will be applied.
/// This is only used if deriv > 0. Default is 1.0.
/// @param mode str, optional
/// Must be 'mirror', 'constant', 'nearest', 'wrap' or 'interp'. This
/// determines the type of extension to use for the padded signal to
/// which the filter is applied.  When `mode` is 'constant', the padding
/// value is given by `cval`.  See the Notes for more details on 'mirror',
/// 'constant', 'wrap', and 'nearest'.
/// When the 'interp' mode is selected (the default), no extension
/// is used.  Instead, a degree `polyorder` polynomial is fit to the
/// last `window_length` values of the edges, and this polynomial is
/// used to evaluate the last `window_length // 2` output values.
/// @param cval scalar, optional
/// Value to fill past the edges of the input if `mode` is 'constant'.
/// Default is 0.0.
/// @return y : ndarray, same shape as `x`
/// The filtered data.
/// @note Details on the `mode` options:
///
///     'mirror':
///         Repeats the values at the edges in reverse order. The value
///         closest to the edge is not included.
///     'nearest':
///         The extension contains the nearest input value.
///     'constant':
///         The extension contains the value given by the `cval` argument.
///     'wrap':
///         The extension contains the values from the other end of the array.
///
/// For example, if the input is [1, 2, 3, 4, 5, 6, 7, 8], and
/// `window_length` is 7, the following shows the extended data for
/// the various `mode` options (assuming `cval` is 0)::
///
///     mode       |   Ext   |         Input          |   Ext
///     -----------+---------+------------------------+---------
///     'mirror'   | 4  3  2 | 1  2  3  4  5  6  7  8 | 7  6  5
///     'nearest'  | 1  1  1 | 1  2  3  4  5  6  7  8 | 8  8  8
///     'constant' | 0  0  0 | 1  2  3  4  5  6  7  8 | 0  0  0
///     'wrap'     | 6  7  8 | 1  2  3  4  5  6  7  8 | 1  2  3
///
// .. versionadded:: 0.14.0

inline Eigen::RowVectorXd savgol_filter(Eigen::RowVectorXd x,
                                        int window_length,
                                        int polyorder,
                                        int deriv        = 0,
                                        float delta      = 1.0,
                                        std::string mode = "interp",
                                        double cval      = 0.0)
{
    const auto mode_array =
        std::array<std::string, 5>{"mirror", "constant", "nearest", "interp", "wrap"};
    bool mode_in_flag = std::find(mode_array.begin(), mode_array.end(), mode) != mode_array.end();
    eigen_assert(mode_in_flag && "mode must be 'mirror', 'constant', 'nearest' "
                                 "'wrap' or 'interp'.");
    if (!mode_in_flag)
    {
        return {};
    }

    auto coeffs = savgol_coeffs(window_length, polyorder, deriv, delta);
    if (mode == "interp")
    {
        eigen_assert((window_length <= x.cols()) &&
                     "If mode is 'interp', window_length must be less "
                     "than or equal to the size of x.");
        if (window_length > x.cols())
        {
            return {};
        }
        // Do not pad. Instead, for the elements within `window_length // 2`
        // of the ends of the sequence, use the polynomial that is fitted to
        // the last `window_length` elements.
        Eigen::RowVectorXd y = scipy::ndimage::convolve1d(x, coeffs, "constant");
        fit_edges_polyfit(x, window_length, polyorder, deriv, delta, y);
        return y;
    }
    else
    {
        Eigen::RowVectorXd y = scipy::ndimage::convolve1d(x, coeffs, mode, cval);
        return y;
    }
}

} // namespace signal
} // namespace scipy
